/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "Shapes.h"
#include "owndebug.h"
#include "Object.h"
#include "World.h"
#include "MaterialHandler.h"
#include "OpenGLTextureLoader.hpp"

/************box class*****************/

CBox::CBox(dReal sizeX, dReal sizeY, dReal sizeZ, CObject *ParentObject, 
           dSpaceID SpaceID, dBodyID parentBodyID, const TMaterial *Material, bool IsStatic)
 :CPrimitive(ParentObject, SpaceID, parentBodyID, Material, IsStatic),
  sizeX(sizeX),
  sizeY(sizeY),
  sizeZ(sizeZ)
{
  _GeomID=dCreateBox(0,sizeX,sizeY,sizeZ);
  dGeomTransformSetGeom(GeomID,_GeomID);
  if (!IsStatic && parentBodyID==0)
    {
      dMassSetBox(&Mass,Material->density,sizeX,sizeY,sizeZ);
      dBodySetMass(BodyID,&Mass);
    }

  iTextureLoaded = false; ///< Texture is not loaded yet

}
    
CBox::~CBox()
{
  
}

//******************************************************************************************
//******************************************************************************************


/** Draws a textured box
 *
 */
void CBox::Draw(std::string const& aTextureName)
{

  if(!iTextureLoaded)
    {
      iTextureID = ParentObject->SimWorld->GetOpenGLTextureLoader()->LoadTexturePNG(aTextureName, 1);
      // printf("Texture loaded!\n");
      iTextureLoaded = true;


    }


  dReal M[16];
  ODEToOGL(dGeomGetRotation(this->GeomID), dGeomGetPosition(this->GeomID), M);
  //glPushAttrib(GL_ALL_ATTRIB_BITS);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glMultMatrixf(M);
  //glColor3f(0.9, 0.9, 0.9);
  //glColor3f(Material->look.color[0],Material->look.color[1],Material->look.color[2]);
  drawBoxTextured(this->sizeX,this->sizeY,this->sizeZ, iTextureID);
  glPopMatrix();
  //glPopAttrib();
} 

//******************************************************************************************
//******************************************************************************************
   
void CBox::Draw()
{

  dReal M[16];
  ODEToOGL(dGeomGetRotation(this->GeomID), dGeomGetPosition(this->GeomID), M);
  glPushAttrib(GL_ALL_ATTRIB_BITS);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glMultMatrixf(M);

  glColor3f(Material->look.color[0],Material->look.color[1],Material->look.color[2]);
  float mcolor[3] = {Material->look.color[0], Material->look.color[1], Material->look.color[2]}; 
  glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mcolor);

  drawBox(this->sizeX,this->sizeY,this->sizeZ);
  glPopMatrix();
  glPopAttrib();
}

/***************Sphere class************/
CSphere::CSphere(dReal radius, CObject *ParentObject, dSpaceID SpaceID, 
                 dBodyID parentBodyID, const TMaterial *Material, 
                 bool IsStatic)
 :CPrimitive(ParentObject, SpaceID, parentBodyID, Material, IsStatic),
  radius(radius)
{
  _GeomID=dCreateSphere(0,radius);
  dGeomTransformSetGeom(GeomID,_GeomID);
  if (!IsStatic && parentBodyID==0)
    {
      dMassSetSphere(&Mass,Material->density,radius);
      dBodySetMass(BodyID,&Mass);
    }
}
    
CSphere::~CSphere()
{
  
}
    
void CSphere::Draw()
{
  dReal M[16];
  ODEToOGL(dGeomGetRotation(this->GeomID), dGeomGetPosition(this->GeomID), M);
  glPushAttrib(GL_ALL_ATTRIB_BITS);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glMultMatrixf(M);
  glColor3f(Material->look.color[0],Material->look.color[1],Material->look.color[2]);
  float mcolor[3] = {Material->look.color[0], Material->look.color[1], Material->look.color[2]}; 
  glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mcolor);

  drawSphere(this->radius,PRIMITIVE_DETAIL_LEVEL);
  glPopMatrix();
  glPopAttrib();
}

/***************Cylinder class************/

CCylinder::CCylinder(dReal radius, dReal length, CObject *ParentObject, dSpaceID SpaceID, 
                     dBodyID parentBodyID, const TMaterial *Material, 
                     bool IsStatic)
 :CPrimitive(ParentObject, SpaceID, parentBodyID, Material, IsStatic),
  radius(radius),
  length(length)
{
  _GeomID=dCreateCylinder(0,radius,length);
  dGeomTransformSetGeom(GeomID,_GeomID);
  if (!IsStatic && parentBodyID==0)
    {
      //3 means mass cylinders length axis is aligned along z axis as with geoms
      dMassSetCylinder(&Mass,Material->density,3,radius,length);
      dBodySetMass(BodyID,&Mass);
    }
}
    
CCylinder::~CCylinder()
{
  
}
    
void CCylinder::Draw()
{
  dReal M[16];
  //get body or geom position? answer: use the frickin geom! geom might be placed off from body
  ODEToOGL(dGeomGetRotation(this->GeomID), dGeomGetPosition(this->GeomID), M);
  glPushAttrib(GL_ALL_ATTRIB_BITS);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glMultMatrixf(M);
  glColor3f(Material->look.color[0],Material->look.color[1],Material->look.color[2]);
  float mcolor[3] = {Material->look.color[0], Material->look.color[1], Material->look.color[2]}; 
  glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mcolor);
  drawCylinder(this->radius,this->length, PRIMITIVE_DETAIL_LEVEL);
  glPopMatrix();
  glPopAttrib();
}

/***************Trimesh class************/

CTrimesh::CTrimesh(float* vertices,int vertexCount,int* indices,int indexCount, 
                   CObject *ParentObject, dSpaceID SpaceID, dBodyID parentBodyID, 
                   const TMaterial *Material, bool isStatic)
 :CPrimitive(ParentObject, SpaceID, parentBodyID, Material, isStatic),
  vertexCount(vertexCount),
  indexCount(indexCount),
  normalCount(indexCount/3), //1 normal per face
  vertices(vertices),
  indices(indices),
  normals(NULL),
  displayList(0)
{
  normals=new float[normalCount*3]; //3 components per normal
  calculateNormals();
  tmDataID=dGeomTriMeshDataCreate();
  
  dGeomTriMeshDataBuildSingle(tmDataID, vertices, 3*sizeof(float), vertexCount,
                              indices, indexCount, 3*sizeof(int));
  
  _GeomID=dCreateTriMesh(0, tmDataID, NULL, NULL, NULL);
  dGeomTransformSetGeom(GeomID,_GeomID);
}

CTrimesh::~CTrimesh()
{
  dGeomTriMeshDataDestroy(tmDataID);
  
  if (normals)
    delete [] normals;

}

void CTrimesh::Draw()
{
  //TODO: calculate normals
  
  dReal M[16];
  ODEToOGL(dGeomGetRotation(this->GeomID), dGeomGetPosition(this->GeomID), M);
  //glPushAttrib(GL_ALL_ATTRIB_BITS);
  glPushAttrib(GL_TRANSFORM_BIT);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glMultMatrixf(M);
      
  if (displayList)
    {
      glCallList(displayList);
    } else {
    //dPrint(1,"generating list");
    displayList=glGenLists(1);
    glNewList(displayList,GL_COMPILE_AND_EXECUTE);
    glBegin(GL_TRIANGLES);
        
    glColor3f(Material->look.color[0],Material->look.color[1],Material->look.color[2]);
    float mcolor[3] = {Material->look.color[0], Material->look.color[1], Material->look.color[2]}; 
    glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mcolor);

        
    for (int i=0;i<normalCount;i++)
      {
        glNormal3f(normals[i*3],normals[i*3+2],normals[i*3+1]);
          
        glVertex3f(vertices[indices[i*3]*3],vertices[indices[i*3]*3+1],vertices[indices[i*3]*3+2]);
        glVertex3f(vertices[indices[i*3+1]*3],vertices[indices[i*3+1]*3+1],vertices[indices[i*3+1]*3+2]);
        glVertex3f(vertices[indices[i*3+2]*3],vertices[indices[i*3+2]*3+1],vertices[indices[i*3+2]*3+2]);
      }
              
    glEnd();
    glColor3f(1.0,0,0);
    glPointSize(2);
    /*glBegin(GL_POINTS);
      for (int i=0;i<vertexCount;i++)
      {  
      glVertex3f(vertices[i*3],vertices[i*3+1],vertices[i*3+2]);
      }
      glEnd();*/
        
    glEndList();
  }
  glPopMatrix();
  glPopAttrib();
};

void CTrimesh::calculateNormals()
{
  float normal[3];
  float v1[3];
  float v2[3];
  float length;
  for (int i=0;i<normalCount;i++)
    {
      v1[0]=vertices[indices[i*3]*3]-vertices[indices[i*3+1]*3];
      v1[1]=vertices[indices[i*3]*3+1]-vertices[indices[i*3+1]*3+1];
      v1[2]=vertices[indices[i*3]*3+2]-vertices[indices[i*3+1]*3+2];
    
      v2[0]=vertices[indices[i*3+2]*3]-vertices[indices[i*3+1]*3];
      v2[1]=vertices[indices[i*3+2]*3+1]-vertices[indices[i*3+1]*3+1];
      v2[2]=vertices[indices[i*3+2]*3+2]-vertices[indices[i*3+1]*3+2];
    
      CrossProduct(v2,v1,normal);
      length=VectorLength(normal);
      normals[i*3]  =normal[0]/length;
      normals[i*3+1]=normal[1]/length;
      normals[i*3+2]=normal[2]/length;
    }
  
}

/***************Plane class************/

CPlane::CPlane(float x1, float y1, float x2, float y2, CObject *ParentObject, 
               dSpaceID SpaceID, const TMaterial *Material)
 :CPrimitive(ParentObject, SpaceID, 0, Material, true),
  x1(x1),
  y1(y1),
  x2(x2),
  y2(y2)
{  
  dGeomDestroy(GeomID);
  
  GeomID=dCreatePlane (SpaceID, 0, 1, 0, 0);
  
  dGeomSetData(this->GeomID,this->GeomData);
  
}
    
CPlane::~CPlane()
{
  
}
    
void CPlane::Draw()
{
  glPushAttrib(GL_ALL_ATTRIB_BITS);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glBegin(GL_QUADS);  
    
  glColor3f(Material->look.color[0],Material->look.color[1],Material->look.color[2]);
  float mcolor[3] = {Material->look.color[0], Material->look.color[1], Material->look.color[2]}; 
  glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mcolor);

  
  glNormal3f(0,1,0);
  glVertex3f(x2,0,y1);
  glVertex3f(x1,0,y1);
  glVertex3f(x1,0,y2);
  glVertex3f(x2,0,y2);
        
  glEnd();
      
  glPopMatrix();
  glPopAttrib();
}

/***************TexturedPlane class************/





CTexturedPlane::CTexturedPlane(float x1, float y1, float x2, float y2, CObject *ParentObject, 
                               dSpaceID SpaceID, const TMaterial *Material)
 :CPrimitive(ParentObject, SpaceID, 0, Material, true),
  x1(x1),
  y1(y1),
  x2(x2),
  y2(y2)
{  
  dGeomDestroy(GeomID);
  
  GeomID=dCreatePlane (SpaceID, 0, 1, 0, 0);
  
  dGeomSetData(this->GeomID,this->GeomData);


  iTextureLoaded = false; ///< Texture has not yet been loaded

  
}
    
CTexturedPlane::~CTexturedPlane()
{
  
}


void CTexturedPlane::LoadTexture(std::string aTextureName)
{
  // We initialize the texture loader
  COpenGLTextureLoader loader;

  // We load the texture given as a parameter
  // printf("Trying to load texture: %s\n", aTextureName);
  //iTextureID = loader.LoadTexturePNG(aTextureName, 1);
  
  iTextureID = ParentObject->SimWorld->GetOpenGLTextureLoader()->LoadTexturePNG(aTextureName, 1);
  // printf("Texture loaded!\n");
  iTextureLoaded = true;


}
    
void CTexturedPlane::Draw()
{

  // We load the texture if it has not yet been loaded
  if(!iTextureLoaded)
    {
      //printf("TEXTURE ID BEFORE: %d\n", iTextureID);
      LoadTexture(std::string("models/textures/concrete.png"));
      //LoadTexture("models/textures/test.png");
      //printf("TEXTURE ID AFTER: %d\n", iTextureID);

    }



  glPushAttrib(GL_ALL_ATTRIB_BITS);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();

  glEnable (GL_TEXTURE_2D);
  glBindTexture (GL_TEXTURE_2D, iTextureID); ///< We bind the texture already loaded
    
    
  //printf("x1: %lf x2: %lf y1: %lf y2: %lf\n", x1, x2, y1, y2);
  glBegin(GL_QUADS);  
    
  glColor3f(0.9, 0.9, 0.9);
  //glColor3f(Material->look.color[0],Material->look.color[1],Material->look.color[2]);
    
  glNormal3f(0,1,0);
    
  //glTexCoord2f (0.0, 0.0);
  glTexCoord2f (0.0, 10.0);
  glVertex3f(x2,0,y1);
  // glTexCoord2f (10.0, 0.0);
  glTexCoord2f (10.0, 10.0);
  glVertex3f(x1,0,y1);
  //glTexCoord2f (10.0, 10.0);
  glTexCoord2f (10.0, 0.0);
  glVertex3f(x1,0,y2);
  // glTexCoord2f (0.0, 10.0);
  glTexCoord2f (0.0, 0.0);
  glVertex3f(x2,0,y2);
    
    
  glEnd();
  glDisable (GL_TEXTURE_2D);
      
  glPopMatrix();
  glPopAttrib();
}

